#include <unistd.h>
#include <error.h>
#include <errno.h>
#include <fcntl.h>
#include <sys/ioctl.h>
#include <sys/types.h>
#include <pthread.h>
#include <linux/videodev2.h>
#include <sys/mman.h>
#include <stdio.h>
#include <stdlib.h>
#include <iostream>

#include "stream_puller_usb.h"

namespace stream {

RKStreamPullerUSB::RKStreamPullerUSB(StreamPullerInfo* spinfo, LogInfo* log_info):
    BasePuller(spinfo, log_info) {
}

RKStreamPullerUSB::~RKStreamPullerUSB() {

}

int RKStreamPullerUSB::get_frame(stream::ImageBlob& buffer) {
  int ret = 0;
  queue_lock.lock();
  if (queue_data.empty()) { queue_lock.unlock(); return -1; }
  buffer = queue_data.front();
  queue_data.pop();
  queue_lock.unlock();
  return 0;
}

int RKStreamPullerUSB::release_mpp(int ret) {
	// if (packet) {
	// 	mpp_packet_deinit(&packet);
	// 	packet = NULL;
	// }

	// if (frame) {
	// 	mpp_frame_deinit(&frame);
	// 	frame = NULL;
	// }

	if (ctx) {
		mpp_destroy(ctx);
		ctx = NULL;
	}

	if (pkt_grp) {
		mpp_buffer_group_put(pkt_grp);
		pkt_grp = NULL;
	}

	if (frm_grp) {
		mpp_buffer_group_put(frm_grp);
		frm_grp = NULL;
	}
  return ret;
}

int RKStreamPullerUSB::init_mpp() {
  int ret = 0;
  // release_mpp(0);
  
  // 初始化
	ret = mpp_create(&ctx, &mpi);
	if (MPP_OK != ret) {
		printf("mpp_create failed\n");
    return release_mpp(-1);
	}


  ret = mpp_init(ctx, MPP_CTX_DEC, (MppCodingType)encode_type);
	if (MPP_OK != ret) {
		printf("mpp_init failed\n");
    return release_mpp(-1);
	}


  return ret;
}

int RKStreamPullerUSB::release_rga(int ret){
  if (src_handle)
      releasebuffer_handle(src_handle);
  if (dst_handle)
      releasebuffer_handle(dst_handle);

  dma_buf_free(dst_buf_size, &dst_dma_fd, (void *)dst_buf);
  dma_buf_free(src_buf_size, &src_dma_fd, (void *)src_buf);
  return ret;
}

int RKStreamPullerUSB::init_rga(){
  int ret = 0;
  memset(&src_img, 0, sizeof(src_img));
  memset(&dst_img, 0, sizeof(dst_img));

  src_format = RK_FORMAT_YCbCr_420_SP;
  dst_format = RK_FORMAT_BGR_888;
  src_buf_size = puller_ifo->oriWidth * puller_ifo->oriHeight * get_bpp_from_format(src_format);
  dst_buf_size = puller_ifo->oriWidth * puller_ifo->oriHeight * get_bpp_from_format(dst_format);
  printf("copy_buffer src_buf_size:%d,dst_buf_size:%d\n",src_buf_size,dst_buf_size);

  ret = dma_buf_alloc(DMA_HEAP_DMA32_UNCACHED_PATH, src_buf_size, &src_dma_fd, (void **)&(src_buf));
  printf("alloc src dma_buf_alloc ret:%d\n", ret);
  if (ret < 0) {
    printf("alloc src dma_heap buffer failed!\n");
    return -1;
  }

  ret = dma_buf_alloc(DMA_HEAP_DMA32_UNCACHED_PATH, dst_buf_size, &dst_dma_fd, (void **)&(dst_buf));
  printf("alloc dst dma_buf_alloc ret:%d\n", ret);
  if (ret < 0) {
    printf("alloc dst dma_heap buffer failed!\n");
    dma_buf_free(src_buf_size, &src_dma_fd, (void*)src_buf);
    return -1;
  }

  src_handle = importbuffer_fd(src_dma_fd, src_buf_size);
  dst_handle = importbuffer_fd(dst_dma_fd, dst_buf_size);
  printf("src_handle:%d, dst_handle:%d\n", src_handle, dst_handle);
  if (src_handle <= 0 || dst_handle <= 0) {
    printf("importbuffer failed!\n");
    return release_rga(-1);
  }
  
  src_img = wrapbuffer_handle(src_handle, puller_ifo->oriWidth, puller_ifo->oriHeight, src_format);
  dst_img = wrapbuffer_handle(dst_handle, puller_ifo->oriWidth, puller_ifo->oriHeight, dst_format);

  imsetColorSpace(&src_img, IM_YUV_BT709_LIMIT_RANGE);
  imsetColorSpace(&dst_img, IM_RGB_FULL);

  return 0;
}
int RKStreamPullerUSB::init_params() {
  int ret = 0;
  if (puller_ifo->enType == AV_CODEC_ID_MJPEG) {encode_type = MPP_VIDEO_CodingMJPEG;}
  else {return -1;}

  // init
  if (0 != (ret = init_device())) {return ret;}

  if (0 != (ret = init_rockchip())) {return ret;}

  stop_flag = common::CameraOpened;
  return 0;
}

int RKStreamPullerUSB::init_device() {
  int ret = 0;
  // initbackFrame
  if (0 != (ret = openDevice())) {return ret;}
  ret = initDevice();
  ret = startCapture();
  if (ret != 0) {return ret;}

  return 0;
}
int RKStreamPullerUSB::init_rockchip() {
  int ret = 0;
  MppDecCfg cfg       = NULL;
  RK_U32 need_split   = 1;

  ret = init_mpp();
  if (ret != 0) {return ret;}

  mpp_dec_cfg_init(&cfg);
  /* get default config from decoder context */
  ret = mpi->control(ctx, MPP_DEC_GET_CFG, cfg);
  if (ret) {
      printf("%p failed to get decoder cfg ret %d\n", ctx, ret);
  }

  /*
  * split_parse is to enable mpp internal frame spliter when the input
  * packet is not aplited into frames.
  */
  ret = mpp_dec_cfg_set_u32(cfg, "base:split_parse", need_split);
  if (ret) {
      printf("%p failed to set split_parse ret %d\n", ctx, ret);
  }

  MppFrameFormat  format;
  // format = MPP_FMT_RGB888;
  // format = MPP_FMT_YUV420P;
  format = MPP_FMT_YUV420SP;
  /* NOTE: change output format before jpeg decoding */
  if (MPP_FRAME_FMT_IS_YUV(format) || MPP_FRAME_FMT_IS_RGB(format)) {
      MPP_RET ret = mpi->control(ctx, MPP_DEC_SET_OUTPUT_FORMAT, &format);
      if (ret) {
          printf("Failed to set output format 0x%x\n", format);
          return NULL;
      }
  }

  ret = mpi->control(ctx, MPP_DEC_SET_CFG, cfg);
  if (ret) {
      printf("%p failed to set cfg %p ret %d\n", ctx, cfg, ret);
  }


  ret = init_rga();
  if (ret != 0) {return ret;}

  return 0;
}

int RKStreamPullerUSB::start_pull() {
  int ret = 0;
  
  while (true) {
    ret = start_pull_imp();

    while (true) {
      std::this_thread::sleep_for(milliseconds(200));
      if (bf::exists(puller_ifo->streamAddress)){
        printf("bf::exists(puller_ifo->streamAddress)\n");
        break;
      }
      spdlog::get("logger")->info("bf::exists(puller_ifo->streamAddress) ");
    }
    if (0 != (ret = init_device())) {
      printf("init_device ret != 0\n");
      break;
    }

  }

  return 0;

}

int RKStreamPullerUSB::start_pull_imp() {
  RK_U32 pkt_done = 0;
	RK_U32 pkt_eos = 0;
	RK_U32 err_info = 0;
	int ret = MPP_OK;
	RK_U32 nPutErrNum = 0;;
	// use av_packet->data initialize mpp packet

  unsigned char *yuv422frame = NULL;
  unsigned long yuvframeSize = 0;
  int mmz_size = puller_ifo->oriWidth * puller_ifo->oriHeight * 4;

  int size_y = puller_ifo->oriWidth * puller_ifo->oriHeight;
  int size_uv = size_y / 2;
  std::vector<char> out_buf(size_y+size_uv);

  int frame_cnt = 0;
  // int64_t timepoint = duration_cast<milliseconds>(system_clock::now().time_since_epoch()).count();;
  int idx = 0;
  float vdec_fps = 0;
  int64_t cur_time = 0;
  int vdec_cnt = 0;
  int64_t start_time = 0, end_time = 0;
  RK_U32 frm_eos = 0;
  MppPacket packet = NULL;
  MppTask task = NULL;
  int break_flag = 0;
  tmp_cnt = 0;

  MppFrame frame = NULL;
  MppBuffer frm_buf   = NULL;
  DecBufMgr       buf_mgr;
  MppBufferGroup  frm_grp;

  printf("mpi_dec start w:%d, h:%d, type:%d\n", puller_ifo->oriWidth, puller_ifo->oriHeight, encode_type);


  ret = dec_buf_mgr_init(&buf_mgr);
  if (ret) {
      printf("dec_buf_mgr_init failed\n");
  }
  
  RK_U32 hor_stride = MPP_ALIGN(puller_ifo->oriWidth, 16);
  RK_U32 ver_stride = MPP_ALIGN(puller_ifo->oriHeight, 16);

  ret = mpp_frame_init(&frame); /* output frame */
  if (ret) {
      printf("mpp_frame_init failed\n");
  }

  frm_grp = dec_buf_mgr_setup(buf_mgr, hor_stride * ver_stride * 4, 4, MPP_DEC_BUF_HALF_INT);
  if (!frm_grp) {
      printf("failed to get buffer group for input frame ret %d\n", ret);
      ret = MPP_NOK;
  }

  /*
    * NOTE: For jpeg could have YUV420 and YUV422 the buffer should be
    * larger for output. And the buffer dimension should align to 16.
    * YUV420 buffer is 3/2 times of w*h.
    * YUV422 buffer is 2 times of w*h.
    * So create larger buffer with 2 times w*h.
    */
  ret = mpp_buffer_get(frm_grp, &frm_buf, hor_stride * ver_stride * 4);
  if (ret) {
      printf("failed to get buffer for input frame ret %d\n", ret);
  }

  mpp_frame_set_buffer(frame, frm_buf);



  int pts = 0;
  stream::ImageBlob buffer(stream::ImageBlobMode_BGR);
  stop_flag = common::CameraGrabing;
  while (stop_flag == common::CameraGrabing) {
    std::this_thread::sleep_for(std::chrono::milliseconds(3));
    int try_times = 3;
    MppBuffer input_buf = NULL;

    ret = getFrame((void **) &yuv422frame, (size_t *)&yuvframeSize);
    if (ret != 0) {break_flag=1; goto free;}
    if (yuvframeSize >= mmz_size) { 
      printf("yuvframeSize >= mmz_size\n");
      continue; 
    }

    ret = mpp_buffer_get(frm_grp, &input_buf, yuvframeSize);
    if (ret != MPP_OK) { printf("allocate input picture buffer failed\n"); }
    memcpy((RK_U8 *)mpp_buffer_get_ptr(input_buf), yuv422frame, yuvframeSize);
    ret = mpp_packet_init_with_buffer(&packet, input_buf);


    ret = mpi->poll(ctx, MPP_PORT_INPUT, MPP_POLL_BLOCK);
    if (ret) { printf("%p mpp input poll failed\n", ctx); }

    ret = mpi->dequeue(ctx, MPP_PORT_INPUT, &task);  /* input queue */
    if (ret) { printf("%p mpp task input dequeue failed\n", ctx); }

    mpp_assert(task);
    ret = mpp_task_meta_set_packet(task, KEY_INPUT_PACKET, packet);
    ret = mpp_task_meta_set_frame (task, KEY_OUTPUT_FRAME,  frame);


    ret = mpi->enqueue(ctx, MPP_PORT_INPUT, task);  /* input queue */
    if (ret) { printf("%p mpp task input enqueue failed\n", ctx);}

    /* poll and wait here */
    ret = mpi->poll(ctx, MPP_PORT_OUTPUT, MPP_POLL_BLOCK);
    if (ret) { printf("%p mpp output poll failed\n", ctx);}

    ret = mpi->dequeue(ctx, MPP_PORT_OUTPUT, &task); /* output queue */
    if (ret) { printf("%p mpp task output dequeue failed\n", ctx); }

    mpp_assert(task);
    if (task) {
        MppFrame frame_out = NULL;
        ret = mpp_task_meta_get_frame(task, KEY_OUTPUT_FRAME, &frame_out);
        if (ret) { printf("mpp_task_meta_get_frame ret:%d\n", ret); }

        /* output queue */
        ret = mpi->enqueue(ctx, MPP_PORT_OUTPUT, task);
        if (ret) {printf("%p mpp task output enqueue failed\n", ctx);}
    }


    if (frame) {
      ret = copy_buffer(frame,buffer.img,out_buf);
      if(ret != 0){ printf("copy_buffer failed:%d\n", ret); }
    }

    if (buffer.img.empty()) {goto free;}
    vdec_cnt++;
    queue_lock.lock();
    if (queue_data.size() >= buffer_cnt) { queue_data.pop(); }
    queue_data.push(buffer);
    queue_lock.unlock();

free:
    if (packet) {mpp_packet_deinit(&packet);packet=nullptr;}
    if (input_buf) {mpp_buffer_put(input_buf);input_buf=nullptr;}
    backFrame();
    if (break_flag) {printf("break\n");break;}

    // 输出fps
    end_time = duration_cast<milliseconds>(system_clock::now().time_since_epoch()).count();
    if (end_time - start_time > 5 * 1e3) {
      vdec_fps = 1.0 / ((end_time - start_time) / (vdec_cnt * 1e3));
      printf("vdec_fps c:%d : %f, vdec_cnt:%d\n", puller_ifo->vdecChn, vdec_fps, vdec_cnt);
      spdlog::get("logger")->info("RKStreamPullerNET::start_pull. vdec_fps c:{} : {}, vdec_cnt:{}", puller_ifo->channelId, vdec_fps, vdec_cnt);
      vdec_cnt = 0;
      start_time = end_time;
    }
  }

  printf("release_ffmpeg\n");
  int stop_cnt = 5;
  while (stop_cnt-- > 0) {
    if (0 == (ret = stopCapture())) {break;}
    std::this_thread::sleep_for(std::chrono::milliseconds(100));
    printf("ERROR. stopCapture != 0\n");
  }
  freeBuffers();
  closeDevice();
  stop_flag = common::CameraClosed;
  return 0;

}


int RKStreamPullerUSB::copy_buffer(MppFrame &frame,cv::Mat& rgbImg,std::vector<char>& out_buf){
  RK_U32 width = 0;
  RK_U32 height = 0;
  RK_U32 h_stride = 0;
  RK_U32 v_stride = 0;
  MppBuffer buffer = NULL;
  RK_U8 *base = NULL;
  int ret = 0;

  // = mpp_frame_get_fmt;
  width = mpp_frame_get_width(frame);
  height = mpp_frame_get_height(frame);
  h_stride = mpp_frame_get_hor_stride(frame);
  v_stride = mpp_frame_get_ver_stride(frame);
  buffer = mpp_frame_get_buffer(frame);

  if(width <= 0 || height <= 0 || NULL == buffer){printf("copy_buffer error return width:%d,height:%d\n",width,height);return -1;}
  base = (RK_U8 *)mpp_buffer_get_ptr(buffer);
  RK_U32 buf_size = mpp_frame_get_buf_size(frame);
  size_t base_length = mpp_buffer_get_size(buffer);
  if(base_length <= 0 || buf_size <= 0 || NULL == base){printf("copy_buffer  error return base_length:%d\n",base_length);return -1;}
  // printf("buf_size: %d  , base_length:%d\n",buf_size,base_length); // 4177920
  // printf("h_stride: %d  , v_stride:%d\n",h_stride,v_stride); // 1920 / 1088
  //存储yuv420sp

  RK_U32 i;
  RK_U8 *base_y = base;
  RK_U8 *base_c = base + h_stride * v_stride;
  int idx = 0;
  for (i = 0; i < height; i++, base_y += h_stride) {
    // fwrite(base_y, 1, width, fp);
    memcpy(out_buf.data() + idx, base_y, width);
    idx += width;
  }
  for (i = 0; i < height / 2; i++, base_c += h_stride) {
    // fwrite(base_c, 1, width, fp);
    memcpy(out_buf.data() + idx, base_c, width);
    idx += width;
  }
  // printf("out_buf size:%d\n",out_buf.size());
  int64_t time1 = duration_cast<milliseconds>(system_clock::now().time_since_epoch()).count();;

  // 使用rga
  memcpy((void*)src_buf, (void*)out_buf.data(), out_buf.size());
  // memset(dst_buf, 0x80, dst_buf_size);

  ret = imcheck(src_img, dst_img, {}, {});
  if (IM_STATUS_NOERROR != ret) {
    printf("%d, check error! %s", __LINE__, imStrError((IM_STATUS)ret));
    return release_rga(-1);
  }

  ret = imcvtcolor(src_img, dst_img, src_format, dst_format);
  if (ret != IM_STATUS_SUCCESS) {
    printf("running failed, %s\n", imStrError((IM_STATUS)ret));
    return release_rga(-1);
  }

  // cv::Mat bgr_img;
  rgbImg = cv::Mat(puller_ifo->oriHeight,puller_ifo->oriWidth,CV_8UC3,dst_buf);
  // 取消拷贝操作 在外层队列入队的时候拷贝
  // rgbImg = bgr_img.clone();


  // 使用opencv
  // cv::Mat yuvImg = cv::Mat(height*3/2,width,CV_8UC1,out_buf.data());
	// cv::cvtColor(yuvImg, rgbImg, CV_YUV2BGR_NV12);
  int64_t time2 = duration_cast<milliseconds>(system_clock::now().time_since_epoch()).count();;

  total_yuv2rgb_time += time2 - time1;
  total_yuv2rgb_cnt +=1;
  if (total_yuv2rgb_cnt % 100 == 0) {
    std::cout << "yuv to rgb time: "<< total_yuv2rgb_time / total_yuv2rgb_cnt << std::endl;
    total_yuv2rgb_time = 0;
    total_yuv2rgb_cnt = 0;
    // std::string temp_path = std::to_string(tmp_cnt)+".png";
    // imwrite(temp_path, rgbImg);
  }
  return 0;
}


int RKStreamPullerUSB::openDevice() {
  /*设备的打开*/
  printf("video dev : %s\n", puller_ifo->streamAddress.c_str());
  fd_cam = open(puller_ifo->streamAddress.c_str(), O_RDWR);
  if (fd_cam < 0) {
    perror("Can't open video device");
    return fd_cam;
  }
  return 0;
}

int RKStreamPullerUSB::closeDevice() {
  if (fd_cam > 0) {
    int ret = 0;
    if ((ret = close(fd_cam)) < 0) {
      perror("Can't close video device");
    }
    return 0;
  } else {
    return -1;
  }
}

int RKStreamPullerUSB::initDevice() {
  int ret;
  struct v4l2_capability cam_cap;    //显示设备信息
  struct v4l2_cropcap cam_cropcap;  //设置摄像头的捕捉能力
  struct v4l2_fmtdesc cam_fmtdesc;  //查询所有支持的格式：VIDIOC_ENUM_FMT
  struct v4l2_crop cam_crop;      //图像的缩放
  struct v4l2_format cam_format;    //设置摄像头的视频制式、帧格式等

  /* 使用IOCTL命令VIDIOC_QUERYCAP，获取摄像头的基本信息*/
  ret = ioctl(fd_cam, VIDIOC_QUERYCAP, &cam_cap);
  if (ret < 0) {
    perror("Can't get device information: VIDIOCGCAP");
  }
  printf(
      "Driver Name:%s\nCard Name:%s\nBus info:%s\nDriver Version:%u.%u.%u\n",
      cam_cap.driver, cam_cap.card, cam_cap.bus_info,
      (cam_cap.version >> 16) & 0XFF, (cam_cap.version >> 8) & 0XFF,
      cam_cap.version & 0XFF);

  /* 使用IOCTL命令VIDIOC_ENUM_FMT，获取摄像头所有支持的格式*/
  cam_fmtdesc.index = 0;
  cam_fmtdesc.type = V4L2_BUF_TYPE_VIDEO_CAPTURE;
  printf("Support format:\n");
  while (ioctl(fd_cam, VIDIOC_ENUM_FMT, &cam_fmtdesc) != -1) {
    printf("\t%d.%s\n", cam_fmtdesc.index + 1, cam_fmtdesc.description);
    cam_fmtdesc.index++;
  }

  /* 使用IOCTL命令VIDIOC_CROPCAP，获取摄像头的捕捉能力*/
  cam_cropcap.type = V4L2_BUF_TYPE_VIDEO_CAPTURE;
  if (0 == ioctl(fd_cam, VIDIOC_CROPCAP, &cam_cropcap)) {
    printf("Default rec:\n\tleft:%d\n\ttop:%d\n\twidth:%d\n\theight:%d\n",
        cam_cropcap.defrect.left, cam_cropcap.defrect.top,
        cam_cropcap.defrect.width, cam_cropcap.defrect.height);
    /* 使用IOCTL命令VIDIOC_S_CROP，获取摄像头的窗口取景参数*/
    cam_crop.type = V4L2_BUF_TYPE_VIDEO_CAPTURE;
    cam_crop.c = cam_cropcap.defrect;    //默认取景窗口大小
    if (-1 == ioctl(fd_cam, VIDIOC_S_CROP, &cam_crop)) {
      //printf("Can't set crop para\n");
    }
  } else {
    printf("Can't set cropcap para\n");
  }

  /* 使用IOCTL命令VIDIOC_S_FMT，设置摄像头帧信息*/
  cam_format.type = V4L2_BUF_TYPE_VIDEO_CAPTURE;
  cam_format.fmt.pix.width = puller_ifo->oriWidth;
  cam_format.fmt.pix.height = puller_ifo->oriHeight;
  cam_format.fmt.pix.pixelformat = V4L2_PIX_FMT_MJPEG;    //要和摄像头支持的类型对应
  cam_format.fmt.pix.field = V4L2_FIELD_INTERLACED;
  ret = ioctl(fd_cam, VIDIOC_S_FMT, &cam_format);
  if (ret < 0) {
    perror("Can't set frame information");
  }
  /* 使用IOCTL命令VIDIOC_G_FMT，获取摄像头帧信息*/
  cam_format.type = V4L2_BUF_TYPE_VIDEO_CAPTURE;
  ret = ioctl(fd_cam, VIDIOC_G_FMT, &cam_format);
  if (ret < 0) {
    perror("Can't get frame information");
  }
  printf("Current data format information:\n\twidth:%d\n\theight:%d\n",
      cam_format.fmt.pix.width, cam_format.fmt.pix.height);
  ret = initBuffers();
  if (ret < 0) {
    perror("Buffers init error");
    //exit(-1);
  }
  return 0;
}

int RKStreamPullerUSB::initBuffers() {
  int ret;
  /* 使用IOCTL命令VIDIOC_REQBUFS，申请帧缓冲*/
  struct v4l2_requestbuffers req;
  CLEAR(req);
  req.count = 3;
  req.type = V4L2_BUF_TYPE_VIDEO_CAPTURE;
  req.memory = V4L2_MEMORY_MMAP;
  ret = ioctl(fd_cam, VIDIOC_REQBUFS, &req);
  if (ret < 0) {
    perror("Request frame buffers failed");
  }
  if (req.count < 2) {
    perror("Request frame buffers while insufficient buffer memory");
  }
  buffers = (struct cam_buffer*) calloc(req.count, sizeof(*buffers));
  if (!buffers) {
    perror("Out of memory");
  }
  for (n_buffers = 0; n_buffers < req.count; n_buffers++) {
    struct v4l2_buffer buf;
    CLEAR(buf);
    // 查询序号为n_buffers 的缓冲区，得到其起始物理地址和大小
    buf.type = V4L2_BUF_TYPE_VIDEO_CAPTURE;
    buf.memory = V4L2_MEMORY_MMAP;
    buf.index = n_buffers;
    ret = ioctl(fd_cam, VIDIOC_QUERYBUF, &buf);
    if (ret < 0) {
      printf("VIDIOC_QUERYBUF %d failed\n", n_buffers);
      return -1;
    }
    buffers[n_buffers].length = buf.length;
    //printf("buf.length= %d\n",buf.length);
    // 映射内存
    buffers[n_buffers].start = mmap(
        NULL, // start anywhere
        buf.length, PROT_READ | PROT_WRITE, MAP_SHARED, fd_cam,
        buf.m.offset);
    if (MAP_FAILED == buffers[n_buffers].start) {
      printf("mmap buffer%d failed\n", n_buffers);
      return -1;
    }
    printf("initBuffers %d.\n", n_buffers);
  }
  return 0;
}

int RKStreamPullerUSB::startCapture() {
  unsigned int i;
  for (i = 0; i < n_buffers; i++) {
    struct v4l2_buffer buf;
    CLEAR(buf);
    buf.type = V4L2_BUF_TYPE_VIDEO_CAPTURE;
    buf.memory = V4L2_MEMORY_MMAP;
    buf.index = i;
    if (-1 == ioctl(fd_cam, VIDIOC_QBUF, &buf)) {
      printf("VIDIOC_QBUF buffer%d failed\n", i);
      return -1;
    }
    printf("VIDIOC_QBUF buffer%d ok.\n", i);
  }
  enum v4l2_buf_type type;
  type = V4L2_BUF_TYPE_VIDEO_CAPTURE;
  if (-1 == ioctl(fd_cam, VIDIOC_STREAMON, &type)) {
    printf("VIDIOC_STREAMON error\n");
    return -1;
  }
  printf("VIDIOC_STREAMON ok\n");
  return 0;
}

int RKStreamPullerUSB::stopCapture() {
  enum v4l2_buf_type type;
  type = V4L2_BUF_TYPE_VIDEO_CAPTURE;
  if (-1 == ioctl(fd_cam, VIDIOC_STREAMOFF, &type)) {
    printf("VIDIOC_STREAMOFF error\n");
    return -1;
  }
  return 0;
}

int RKStreamPullerUSB::freeBuffers() {
  unsigned int i;
  for (i = 0; i < n_buffers; ++i) {
    if (-1 == munmap(buffers[i].start, buffers[i].length)) {
      printf("munmap buffer%d failed\n", i);
      return -1;
    }
  }
  free(buffers);
  return 0;
}

int RKStreamPullerUSB::getFrame(void **frame_buf, size_t* len) {
  struct v4l2_buffer queue_buf;
  CLEAR(queue_buf);
  queue_buf.type = V4L2_BUF_TYPE_VIDEO_CAPTURE;
  queue_buf.memory = V4L2_MEMORY_MMAP;
  if (-1 == ioctl(fd_cam, VIDIOC_DQBUF, &queue_buf)) {
    printf("VIDIOC_DQBUF error\n");
    return -1;
  }
  *frame_buf = buffers[queue_buf.index].start;
  *len = buffers[queue_buf.index].length;
  frameIndex = queue_buf.index;
  return 0;
}

int RKStreamPullerUSB::backFrame() {
  if (frameIndex != -1) {
    struct v4l2_buffer queue_buf;
    CLEAR(queue_buf);
    queue_buf.type = V4L2_BUF_TYPE_VIDEO_CAPTURE;
    queue_buf.memory = V4L2_MEMORY_MMAP;
    queue_buf.index = frameIndex;
    if (-1 == ioctl(fd_cam, VIDIOC_QBUF, &queue_buf)) {
      printf("VIDIOC_QBUF error\n");
      return -1;
    }
    return 0;
  }
  return -1;
}

int RKStreamPullerUSB::stop_pull() {
  // if (stop_flag1 == common::CameraGrabing) {
  //   stop_flag1 = common::CameraClosed;
  //   while (stop_flag1 != common::CameraOpened) {
  //     spdlog::get("logger")->info("StreamPullerBase::stop_pull stop_flag1: {} ", stop_flag1);
  //     std::this_thread::sleep_for(milliseconds(50));
  //   }
  // }
  int try_cnt = 200;
  if (stop_flag == common::CameraGrabing) {
    stop_flag = common::CameraOpened;
    while (stop_flag != common::CameraClosed && try_cnt-- > 0) {
      spdlog::get("logger")->info("RKStreamPullerUSB::stop_pull stop_flag: {} ", stop_flag);
      std::this_thread::sleep_for(milliseconds(50));
    }
    if (try_cnt < 0) {exit(0);}
  }
  // release_mpp(0);   // 此操作会导致崩溃
  release_rga(0);
  return 0;
}

}